# -- coding:utf-8 --
#!/usr/bin/python
import sys, os
sys.path.append("/home/avatar/Minibot/04.RobotSamples/01.Arm")
import inspect
from LoadJson import LoadJson
from BwSensorLib import BwSensorLib
from ArmRobot import ArmRobot
from BwRobotLib import BwRobotLib
from MiniBotControlLib import MiniBotControlLib
import time
import BwHelpLib 
import random

# 加载机器人构型配置
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath = os.path.dirname(os.path.abspath(fileName))
config_file = folderPath+"/config.txt"


class RemoteGrab:		
	def __init__(self):
		# 实例化MiniBotControlLib类，并设置为udp模式
		self.mlib = MiniBotControlLib("udp")
		# 输入ip地址和端口号
		self.ip = "10.10.100.254"
		self.port = 8899
		# 连接机器人,并设置超时时间5s
		self.mlib.Connect(self.ip, self.port, 2000)
		# 实例化BwRobotLib类，并将mlib作为参数传入
		self.robotlib = BwRobotLib(self.mlib)
		# 导入配置文件，获取文件中的模块ID和模块连接位置
		self.module_ids, self.module_ids_loc = self.robotlib.loadConfig(config_file)
		# 扫描当前机器人构型的ID，默认扫描ID段为20-50
		self.module_ids_scan, self.module_ids_loc_scan = self.mlib.ScanId()
		# 判断是否扫描的ID与配置文件导入的ID一致
		res = BwHelpLib.checkListEqual(self.module_ids,self.module_ids_scan)
		if res == False:
			print("\033[31m输入的模块ID与扫描获取的模块ID不一致，请检查！\033[37m")
			print("配置文件中模块ID: ", self.module_ids)
			print("实际扫描的模块ID: ", self.module_ids_scan)
			exit(0)
		else:
			print("输入的模块ID与扫描获取的模块ID一致")
			
		# 实例化操作臂类
		self.armlib = ArmRobot(self.mlib)
		# 将模块ID与操作臂构型绑定
		self.armlib.bindIds(self.module_ids)		
		# 实例化传感器库对象
		self.sensorlib = BwSensorLib(self.mlib)
		# 动作延时时间
		self.wait_time = 1.5
		# 动作速度
		self.speed = 50	
		# 点阵显示图案 
		# 图案分别为：向左箭头　向右箭头
		self.display_data = [[0x08,0x10,0x3F,0x10,0x08],[0x08,0x04,0x7E,0x04,0x08],[0x00,0x00,0x00,0x00,0x00]]

		#动作组数据
		self.ZoneAToZoneRed = []
		self.ZoneBToZoneGreen = []
		self.ZoneCToZoneBlue = []
		self.ZoneRedToZoneA = []
		self.ZoneGreenToZoneB = []
		self.ZoneBlueToZoneC = []	

		# 实例化导入动作组数据对象
		self.load_json = LoadJson()
		# A点到红色点的运动序列
		self.ZoneAToZoneRed = self.load_json.LoadActionDataAToRed(
			self.ZoneAToZoneRed)
		# B点到红绿色点的运动序列
		self.ZoneBToZoneGreen = self.load_json.LoadActionDataBToGreen(
			self.ZoneBToZoneGreen)
		# C点到蓝色点的运动序列
		self.ZoneCToZoneBlue = self.load_json.LoadActionDataCToBlue(
			self.ZoneCToZoneBlue)
		# 红色点到A点的运动序列	
		self.ZoneRedToZoneA = self.load_json.LoadActionDataRedToA(
			self.ZoneRedToZoneA)
		# 绿色点到B点的运动序列
		self.ZoneGreenToZoneB = self.load_json.LoadActionDataGreenToB(
			self.ZoneGreenToZoneB)
		# 蓝色点到C点的运动序列
		self.ZoneBlueToZoneC = self.load_json.LoadActionDataBlueToC(
			self.ZoneBlueToZoneC)
		# 初始化动作
		self.initRobotMove()

	# 初始化至实验开始位置
	def initRobotMove(self, _time=2):
		angs = [0,38.6,65.9,63,1.2,1.1]
		speed = 20
		self.armlib.position_angles = angs
		self.armlib.pushJointsAngles(speed, self.wait_time)
		time.sleep(_time)

# ####遥控抓取
	def remoteGrab(self):
		while 1:
			print("请输入按键")
			s1 = self.sensorlib.readRemoteSensor()
			print ("返回值：",s1)
			# 按下 '1'
			if(s1==10):
				print("按下＂1＂－－从A到红")
				self.sensorlib.setMatrixSensor(self.display_data[0]) # 点阵显示左箭头图案
				for b in range(0,10): # 从A点抓夹方块到红色点有10组动作
					self.armlib.position_angles = self.ZoneAToZoneRed[b]
					self.armlib.pushJointsAngles(self.speed,self.wait_time)
					if(b == 6): # 机械臂转至传感器区域，动作幅度较大，设置等待时间
						time.sleep(self.wait_time)
				self.initRobotMove(self.wait_time)	# 回到初始化位置		
			# 按下 '2'		
			elif(s1==11):
				print("按下＂2＂－－从B到绿")
				self.sensorlib.setMatrixSensor(self.display_data[0]) # 点阵显示左箭头图案
				for b in range(0,9): # 从B点抓夹方块到绿色点有9组动作
					self.armlib.position_angles = self.ZoneBToZoneGreen[b]
					self.armlib.pushJointsAngles(self.speed,self.wait_time)
					if(b == 5): # 机械臂转至传感器区域，动作幅度较大，设置等待时间
						time.sleep(self.wait_time)
						time.sleep(1)
				self.initRobotMove(self.wait_time)	# 回到初始化位置
			# 按下 '3'				
			elif(s1==12):
				print("按下＂3＂－－从C到蓝")
				self.sensorlib.setMatrixSensor(self.display_data[0]) # 点阵显示左箭头图案
				for b in range(0,10): # 从C点抓夹方块到蓝色点有10组动作
					self.armlib.position_angles = self.ZoneCToZoneBlue[b]
					self.armlib.pushJointsAngles(self.speed,self.wait_time)
					if(b == 6): # 机械臂转至传感器区域，动作幅度较大，设置等待时间
						time.sleep(self.wait_time)
				self.initRobotMove(self.wait_time)	# 回到初始化位置	
			# 按下 '4'					
			elif(s1==13):
				print("按下＂4＂－－从红到A")
				self.sensorlib.setMatrixSensor(self.display_data[1]) # 点阵显示右箭头图案
				for b in range(0,11): # 从红色点抓夹方块到A点有11组动作
					self.armlib.position_angles = self.ZoneRedToZoneA[b]
					self.armlib.pushJointsAngles(self.speed,self.wait_time)
					if(b == 6): # 机械臂转至传感器区域，动作幅度较大，设置等待时间
						time.sleep(self.wait_time)
						time.sleep(1)
				self.initRobotMove(self.wait_time)	# 回到初始化位置
			# 按下 '5'
			elif(s1==14):
				print("按下＂5＂－－从绿到B")
				self.sensorlib.setMatrixSensor(self.display_data[1]) # 点阵显示右箭头图案
				for b in range(0,9): # 从绿点抓夹方块到B色点有9组动作
					self.armlib.position_angles = self.ZoneGreenToZoneB[b]
					self.armlib.pushJointsAngles(self.speed,self.wait_time)
					if(b == 5): # 机械臂转至传感器区域，动作幅度较大，设置等待时间
						time.sleep(self.wait_time)
						time.sleep(1)
				self.initRobotMove(self.wait_time)	# 回到初始化位置
			# 按下 '6'
			elif(s1==15):
				print("按下＂6＂－－从蓝到C")
				self.sensorlib.setMatrixSensor(self.display_data[1]) # 点阵显示右箭头图案
				for b in range(0,11): # 从蓝点抓夹方块到C点有11组动作
					self.armlib.position_angles = self.ZoneBlueToZoneC[b]
					self.armlib.pushJointsAngles(self.speed,self.wait_time)
					if(b == 6): # 机械臂转至传感器区域，动作幅度较大，设置等待时间
						time.sleep(self.wait_time)
				self.initRobotMove(self.wait_time)	# 回到初始化位置
			# 按下其他按键
			else:
				print("请按下按键")
				self.sensorlib.setMatrixSensor(self.display_data[2]) # 点阵不显示图案
				self.initRobotMove(self.wait_time)	# 回到初始化位置
					

# 主函数
def main():
	# 实例化遥控操作类
	remote = RemoteGrab()
	# 执行遥控操作功能
	remote.remoteGrab()


if __name__ == "__main__":
	try:
		main()
	except KeyboardInterrupt:
		print(" Program closed! ")
	pass
